#include <vector>
#include <stdint.h>
#include <rclcpp/rclcpp.hpp>
#ifndef CRC_CHECKER_HPP
#define CRC_CHECKER_HPP

class CrcChecker {
private:
    std::vector<uint32_t> crc_table;

    // Generate the CRC32 lookup table
    void generate_crc32_table() {
        uint32_t polynomial = 0xEDB88320;
        for (int i = 0; i < 256; i++) {
            uint32_t crc = i;
            for (int j = 0; j < 8; j++) {
                if (crc & 1) {
                    crc = (crc >> 1) ^ polynomial;
                } else {
                    crc >>= 1;
                }
            }
            crc_table.push_back(crc);
        }
    }

public:
    // Constructor
    CrcChecker() {
        generate_crc32_table();
    }

    // Calculate CRC32
    uint32_t calculate_crc(const std::vector<unsigned char>& data, int end_index, int begin_index = 0) {
        uint32_t crc = 0xFFFFFFFF;

        for (int i = begin_index; i < end_index; i++) {
            uint32_t lookup_index = (crc ^ data[i]) & 0xFF;
            crc = (crc >> 8) ^ crc_table[lookup_index];
        }

        return crc ^ 0xFFFFFFFF;
    }

    // Generate a random number
    unsigned char generate_random_number() {
        return static_cast<unsigned char>(rand() % 256);
    }

    bool check_crc(uint32_t crc_raw, const std::vector<uint8_t>& check_data) {
        // Extract the bytes of the CRC
        uint8_t crc_1 = (crc_raw >> 24) & 0xFF;
        uint8_t crc_2 = (crc_raw >> 16) & 0xFF;
        uint8_t crc_3 = (crc_raw >> 8) & 0xFF;
        uint8_t crc_4 = crc_raw & 0xFF;

        // Get the CRC value from check_data
        uint8_t receive_crc_1 = check_data[check_data.size() - 5];
        uint8_t receive_crc_2 = check_data[check_data.size() - 4];
        uint8_t receive_crc_3 = check_data[check_data.size() - 3];
        uint8_t receive_crc_4 = check_data[check_data.size() - 2];

        // Check if each byte matches
        if (crc_4 == receive_crc_4 && crc_3 == receive_crc_3 && crc_2 == receive_crc_2 && crc_1 == receive_crc_1) {
            return true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("crc_checker"), "CRC check failed");
            return false;
        }
    }
};

#endif // CRC_CHECKER_HPP
